#include "Map.h"
#include "World.h"
#include "StaticEntity.h"
#include "ItemGenerator.h"

Map::Map()
{
	map_mesh__ = 0;
	map_node__ = 0;
	selector__ = 0;

	player__ = 0;
}

Map::~Map()
{
	unload();
}

// cast a ray for an entity and return the entity it collides with
Entity* Map::RayCastForEntity(ISceneNode* node, vector3df origin, vector3df direction, s32 IDMask)
{
	triangle3df tr;
	vector3df impact_point;

	line3d<f32> ray(origin, origin + direction);
	ISceneNode* resultNode = World::getSceneManager()->getSceneCollisionManager()->getSceneNodeFromRayBB(ray, IDMask);

	// A node has been touched
	if (resultNode != 0)
	{
		float distToImpact = 10;
		float distToResult = 1;
		bool tst = World::getSceneManager()->getSceneCollisionManager()->getCollisionPoint(ray, selector__, impact_point, tr);
		if (tst)
		{
			distToImpact = node->getPosition().getDistanceFrom(impact_point);
			distToResult = node->getPosition().getDistanceFrom(resultNode->getPosition());
		}

		// no wall in front of the node
		if (distToResult < distToImpact)
			if (entities__.find(resultNode) != entities__.end())
				return entities__[resultNode];
	}

	return 0;
}

std::map<ISceneNode*, Entity*>& Map::getEntitiesMap()
{
	return entities__;
}


// frame processor
void Map::update_entities()
{
	std::map<ISceneNode*, Entity*>::iterator it = entities__.begin();
	while (it != entities__.end())
	{
		it->second->update();
		it++;
	}

	// Now check for special collisions
	std::list<std::map<std::string, std::string> >::iterator it2 = special_entities__.begin();
	while (it2 != special_entities__.end())
	{
		std::map<std::string, std::string>& ent_info = *it2;
		it2++;

		if (ent_info["classname"] == "info_next_level")
		{
			if (BBcollision(player__->getNode(), bspStrToVector3df(ent_info["origin"]), 32))
				if (doCollisionWith(ent_info))
					return; // Must not continue, because a new map will be loaded
		}
	}
}

bool Map::doCollisionWith(std::map<std::string, std::string>& ent_info)
{
	if (ent_info["classname"] == "info_next_level")
	{
		if (ent_info.find("new_hub") != ent_info.end())
			loadToNewHub(ent_info["map_name"].c_str(), ent_info["target"].c_str());
		else
			load(ent_info["map_name"].c_str(), ent_info["target"].c_str());
		return true;
	}

	return false;
}


// load a new part of the level
void Map::loadToNewHub(const irr::core::string<c8>& map_name, const irr::core::string<c8>& target_spawn)
{
	unloadToNewHub();
	load(map_name, target_spawn);
}


// load the mesh of the level
void Map::load_level_mesh(const irr::core::string<c8>& map_name)
{
	ISceneManager* smgr = World::getSceneManager();

	irr::core::string<c8> true_name = "./baseq3/maps/";
	true_name += map_name + ".bsp";

	scene::IQ3LevelMesh* mesh = dynamic_cast<IQ3LevelMesh*>(smgr->getMesh(true_name.c_str()));
	//World::getSceneManager()->getMeshCache->removeMesh(mesh); // Not tested, but may be buggy
	map_mesh__ = mesh->getMesh(quake3::E_Q3_MESH_GEOMETRY);
	if (map_mesh__ == 0)
		throw Exception(std::string("Could not load map: ") + true_name.c_str(), __FILE__, __LINE__);
	
	map_node__ = smgr->addOctTreeSceneNode(map_mesh__);
	if (map_node__ == 0)
	{
		unload_mesh();
		throw Exception("could not add the octree scene node ??? wtf ?", __FILE__, __LINE__);
	}

	map_node__->setName("Octree");
	map_node__->setID(5);

	selector__ = smgr->createOctTreeTriangleSelector(map_mesh__, map_node__, 128);
	if (selector__ == 0)
	{
		unload_node();
		unload_mesh();

		throw Exception("What the hell is this ? Could not create the octree triangle selector !!", __FILE__, __LINE__);
	}
}

void Map::unload_way2go()
{
	zones__.clear();
	path_table__.clear();
	segments__.clear();
}

void Map::load_way2go(const irr::core::string<c8>& map_name)
{
	// TODO: check for read failures at read time
	std::string true_filename = "./baseq3/maps/";
	true_filename += map_name.c_str();
	true_filename += ".w2g";

	std::ifstream is(true_filename.c_str(), std::ios_base::binary | std::ios_base::in);
	if (!is.is_open())
	{
		std::cerr << "WARNING: Could not load file: " << true_filename << std::endl;
		return;
	}
	
	// check magic !!
	{
		std::string s;
		is >> s;

		if (s != "Way2go__File")
		{
			is.close();
			std::cerr << "ERROR: Magic not verified in file: " << true_filename << std::endl;
			return;
		}
	}

	// Read zones
	{
		u32 nb_zones;
		is >> nb_zones;

		// Retrieve all zone informations
		for (unsigned int i = 0; i<nb_zones; i++)
		{
			zone z;

			// Read the zone's name
			is >> z.getName();

			// Read zone's points
			{
				vector3df point;
				for (int j=0; j<4; j++)
				{
					is >> point.X;
					is >> point.Y;
					is >> point.Z;

					z.getPoints().push_back(point);
				}
				z.calculatePlane();

			}

			// Read zone's segments
			{
				u32 nb_segs;
				is >> nb_segs;

				for (unsigned int j=0; j<nb_segs; j++)
				{
					s32 seg_index;
					is >> seg_index;

					z.getSegments().push_back(seg_index);
				}
				
				zones__.push_back(z);
			}

		} // End of: for each zone
	}

	// Read segments
	{
		u32 nb_segs;
		is >> nb_segs;

		for (unsigned int i=0; i<nb_segs; i++)
		{
			segment_t s;

			is >> s.first .X;
			is >> s.first .Y;
			is >> s.first .Z;
			is >> s.second.X;
			is >> s.second.Y;
			is >> s.second.Z;

			segments__.push_back(s);
		}
	}


	// Read path table
	{
		unsigned int nb_segs = segments__.size();

		for (unsigned int start=0; start<nb_segs; start++)
		{
			std::vector<std::pair<int, float> > arrived_vector;

			for (unsigned int end=0; end<nb_segs; end++)
			{
				std::pair<int, float> path_information;

				is >> path_information.first;
				is >> path_information.second;

				arrived_vector.push_back(path_information);
			}

			path_table__.push_back(arrived_vector);
		}
	}

	// oueeeeeee !
	is.close();


}

// load the entities from a file
void Map::load_level_entities(const irr::core::string<c8>& map_name, const irr::core::string<c8>& target_spawn)
{
	bool found_player = false;

	irr::core::string<c8> s = "./baseq3/maps/";
	s += map_name + ".bsp";

	BspEnt ents;
	ents.load(s.c_str());

	std::list<std::map<std::string, std::string> >::iterator it = ents.getNodes().begin();
	while (it != ents.getNodes().end())
	{
		std::map<std::string, std::string>& ent_info = *it;

		if (ent_info["classname"] == "info_player_start" && !found_player)
		{
			if ( (target_spawn == "" && ent_info.find("target") == ent_info.end()) ||
			     (target_spawn == ent_info["target"].c_str()) )
			{
				found_player = true;
				SpawnPlayer(ent_info);
			}
		}
		else if (ent_info["classname"].substr(0, 8) == "monster_")
			SpawnNPC(ent_info);
		else if (ent_info["classname"] == "item_treasure" || ent_info["classname"] == "item_food")
			SpawnItem(ent_info);
		else if (ent_info["classname"] == "item_generator")
			SpawnGenerator(ent_info);
		else if (ent_info["classname"] == "info_next_level")
			special_entities__.push_back(ent_info);
		else if (ent_info["classname"] == "info_patrol")
		{
			zone z;
			std::pair<vector3df, int> p;
			p.first = bspStrToVector3df(ent_info["origin"]);
			//p.second = getZoneFromVector3df(p.first, z);

			patrol_points__.push_back(p);
		}
		else
			std::cout << "Unhandled entity: " << ent_info["classname"] << std::endl;

		it++;
	}

	if (!found_player)
		throw Exception(std::string("No valid entry point in ") + s.c_str(), __FILE__, __LINE__);
}

// Warning: player's group does not support new map loading, unless it is explicitly re set again in this method
void Map::SpawnPlayer(std::map<std::string, std::string>& ent_info)
{
	// Create the player entity if needed
	if (player__ == 0)
	{
		player__ = new PlayerEntity("models/characters/knight/tris.md2", "models/characters/knight/skin.jpg", "models/characters/knight/vwep/tris.md2", "models/characters/knight/vwep/skin.jpg");
		player__->createCamera();
		player__->getNode()->setPosition(bspStrToVector3df(ent_info["origin"]));

		double rotation = ent_info.find("angle") == ent_info.end() ? 0.0: strToDouble(ent_info["angle"]);
		player__->getNode()->setRotation(vector3df(0, -rotation, 0));

		player__->getNode()->setScale(vector3df(2, 2, 2));
		player__->setAnimation("stand");
		player__->getNode()->setID(512);

		World::getInputReceiver()->setPlayer(player__);
		
	}
	else
		player__->getNode()->setPosition(bspStrToVector3df(ent_info["origin"]));

	setCollisionResponseAnimator(player__->getNode(), vector3df(50,50,22),vector3df(0,-5,0),vector3df(0,5,0));

	player__->setGroup("player");

	player__->set3rdPersonView();

	//player__->setZone(getZoneFromVector3df(player__->getNode()->getPosition()));

	entities__[player__->getNode()] = player__;

}

ItemGenerator* Map::SpawnGenerator(std::map<std::string, std::string>& ent_info)
{
	ItemGenerator* gen;

	gen = new ItemGenerator(ent_info["model"], ent_info["skin"], ent_info["child_classname"], ent_info["scale"]);
	gen->getNode()->setPosition( bspStrToVector3df(ent_info["origin"]) );
	gen->setType(ent_info["classname"].c_str());

	if (ent_info.find("max_child") != ent_info.end())
		gen->setMaxChildren( strToInt(ent_info["max_children"]) );
	if (ent_info.find("time_to_spawn") != ent_info.end())
		gen->setTimeToSpawn( strToDouble(ent_info["time_to_spawn"]) );

/*
	zone z;
	if (getZoneFromVector3df(gen->getNode()->getPosition(), z))
		gen->setZone(&z);
*/
	//gen->setZone( getZoneFromVector3df(gen->getNode()->getPosition()) );
	entities__[gen->getNode()] = gen;
	return gen;
}

StaticEntity* Map::SpawnItem(std::map<std::string, std::string>& ent_info)
{
	StaticEntity* item;

	item = new StaticEntity(ent_info["model"].c_str(), ent_info["skin"].c_str());
	item->getNode()->setPosition( bspStrToVector3df(ent_info["origin"]) );
	item->setType(ent_info["classname"].c_str());

	double rotation = ent_info.find("angle") == ent_info.end() ? 0.0: strToDouble(ent_info["angle"]);
	item->getNode()->setRotation(vector3df(0.0f, -rotation, 0.0f));

	if (ent_info.find("scale") != ent_info.end())
	{
		double s = strToDouble(ent_info["scale"]);
		item->getNode()->setScale(vector3df(s, s, s));
	}

	setCollisionResponseAnimator(item->getNode(), vector3df(4.0, 2.0, 4.0), vector3df(0,-5,0),vector3df(0,0,0));

/*
	zone z;
	if (getZoneFromVector3df(item->getNode()->getPosition(), z))
		item->setZone(&z);
*/
	//item->setZone( getZoneFromVector3df(item->getNode()->getPosition()) );
	entities__[item->getNode()] = item;

	return item;
}

NPCEntity* Map::SpawnNPC(std::map<std::string, std::string>& ent_info)
{
	NPCEntity* Npc;
	
	if (ent_info.find("weaponmodel") == ent_info.end())
		ent_info["weaponmodel"] = "";
	if (ent_info.find("weaponskin") == ent_info.end())
		ent_info["weaponskin"] = "";

	Npc = new NPCEntity(ent_info["behavior"].c_str(), ent_info["model"].c_str(), ent_info["skin"].c_str(), ent_info["weaponmodel"].c_str(), ent_info["weaponskin"].c_str());
	Npc->getNode()->setPosition(bspStrToVector3df(ent_info["origin"]));
	Npc->setType(ent_info["classname"].c_str());

	double rotation = ent_info.find("angle") == ent_info.end() ? 0.0: strToDouble(ent_info["angle"]);
	Npc->getNode()->setRotation(vector3df(0, -rotation, 0));

	if (ent_info.find("scale") != ent_info.end())
	{
		double s = strToDouble(ent_info["scale"]);
		Npc->getNode()->setScale(vector3df(s, s, s));
	}

	std::string attributes_file = ent_info["classname"];
	if (ent_info.find("attributes") != ent_info.end())
		attributes_file = ent_info["attributes"];
	Npc->loadAttributesFromFile(attributes_file);

	Npc->setAnimation(EMAT_STAND);

	vector3df v(0,0,0);
	vector3df vt(0, 5, 0);

	if( ent_info["classname"]=="monster_goblin" )
		v=vector3df(50,50,50);
	else if(  ent_info["classname"]=="monster_spider" )
		v=vector3df(50,50,22);
	else if ( ent_info["classname"] == "monster_froggy" )
	{
		v = vector3df(50.0f, 50.0f, 22.0f);
		vt.Y = 5;
		//std::cout<<"monster_froggy->getAttribute[eatable]="<<Npc->getAttribute(std::string("eatable"))<<std::endl;
		Npc->getAttribute(std::string("eatable")) = 1.0;
		//std::cout<<"monster_froggy->getAttribute[eatable]="<<Npc->getAttribute(std::string("eatable"))<<std::endl;
	}
	else if ( ent_info["classname"] == "monster_bug" )
	{
		v = vector3df(50.0f, 50.0f, 22.0f);
		vt.Y = 20;
	}
	else if ( ent_info["classname"] == "monster_ogre" )
	{
		v = vector3df(50.0f, 50.0f, 22.0f);
		vt.Y = 20;
	}
	else if ( ent_info["classname"] == "monster_bauul" )
	{
		v = vector3df(50.0f, 50.0f, 22.0f);
		vt.Y = 20;
	}


	setCollisionResponseAnimator(Npc->getNode(), v,vector3df(0,-5,0),vt);

	std::string hostilities_file = Npc->getType().c_str();
	if (ent_info.find("hostilities") != ent_info.end())
		hostilities_file = ent_info["hostilities"];
	Npc->getMemory()->getHostilities()->loadFromFile(hostilities_file);

/*
	zone z;
	if (getZoneFromVector3df(Npc->getNode()->getPosition(), z))
		Npc->setZone(&z);
*/
	//Npc->setZone(getZoneFromVector3df(Npc->getNode()->getPosition()));

	entities__[Npc->getNode()] = Npc;

	return Npc;
}

bool Map::RemoveEntity(ISceneNode* entityNode)
{
	Entity* entity;
	if (entities__.find(entityNode) == entities__.end())
		return false;

	entity = entities__[entityNode];

	if (World::getWorld()->getTargeted() == entity)
		World::getWorld()->setTargeted(0);

	// Unregister the entity from all groups of the post office
	PostOffice::UnregisterAllGroups(entity);

	// Remove Entity from map
	std::map<ISceneNode*, Entity*>::iterator it = entities__.find(entityNode);
	if (it != entities__.end())
		entities__.erase(it);
	
	delete entity;
	
	return true;
}

bool Map::RemoveEntity(Entity* ent)
{
	std::map<ISceneNode*, Entity*>::iterator it = entities__.begin();
	std::map<ISceneNode*, Entity*>::iterator it2;

	while (it != entities__.end())
	{
		it2 = it;
		it2++;
		if (it->second == ent)
			return RemoveEntity(it->first);
		it = it2;
	}
	return false;
}

bool Map::EntityExists(Entity* entity)
{
	if(entity != 0)
	{
		std::map<ISceneNode*, Entity*>::iterator it = entities__.begin();
	
		while (it != entities__.end())
		{
			if (it->second == entity)
				return true;
			it++;
		}
		return false;
	}
	else
	{
		return true ;
	}
}

void Map::load(const irr::core::string<c8>& map_name, const irr::core::string<c8>& target_spawn)
{
	unload();

	load_level_mesh(map_name);
	load_way2go(map_name);
	load_level_entities(map_name, target_spawn);

	visited_maps__[map_name] = true;
	current_map__ = visited_maps__.find(map_name);
}

void Map::unloadToNewHub()
{
	// Should remove any temporary files now

	visited_maps__.clear();
	current_map__ = visited_maps__.end();

	unload();
}

void Map::unload_selector()
{
	if (selector__)
	{
		if (selector__->drop() == false)
			std::cout << "WARNING: selector was not truely deleted in Map::unload()" << std::endl;
		selector__ = 0;
	}
}

void Map::unload_node()
{
	if (map_node__)
	{
		map_node__->remove();
		map_node__ = 0;
	}
}

void Map::unload_mesh()
{
	if (map_mesh__)
	{
//		World::getSceneManager()->getMeshCache()->removeMesh(map_mesh__);
		map_mesh__ = 0;
	}
}

void Map::unload()
{
	World* w=World::getWorld();
	w->setTargeted(0);
	w->InitWindowNPC();

	// Drop all entities from PostOffice
	PostOffice::groups__.clear();

	std::map<ISceneNode*, Entity*>::iterator it = entities__.begin();
	std::map<ISceneNode*, Entity*>::iterator it2;
	while (it != entities__.end())
	{
		it2 = it;
		it2++;

		if (it->second == player__)
		{
			it->second->getNode()->removeAnimators();
			it = it2;
			continue;
		}

		// do the dirty job here
		Entity* ent = it->second;
		entities__.erase(it);
		delete ent;

		it = it2;
	}

	unload_selector();
	unload_node();
	unload_mesh();
	unload_way2go();

	current_map__ = visited_maps__.end();
	special_entities__.clear();
	patrol_points__.clear();
}

IMesh* Map::getMesh()
{
	return map_mesh__;
}

ISceneNode* Map::getNode()
{
	return map_node__;
}

ITriangleSelector* Map::getSelector()
{
	return selector__;
}

void Map::setCollisionResponseAnimator(ISceneNode* node,
				       const vector3df& ellipsoidRadius,
				       const vector3df& gravityPerSecond,
				       const vector3df& ellipsoidTranslation,
				       f32 slidingValue)
{
	ISceneNodeAnimator* anim = World::getSceneManager()->createCollisionResponseAnimator(selector__, node, ellipsoidRadius, gravityPerSecond, ellipsoidTranslation, slidingValue);
	node->addAnimator(anim);

	anim->drop();
}

std::vector<std::pair<vector3df, int> >& Map::getPatrolPoints()
{
	return patrol_points__;
}

int Map::getRandomPatrolPoint()
{
	if (patrol_points__.empty())
		return -1;
	else
		return rand()%patrol_points__.size();
}

PlayerEntity* Map::getPlayer()
{
    return player__;
}

float argument(std::complex<float> c)
{
	float ang = std::arg(c) * 180.0f / M_PI;
	
	while (ang < 0.0f)
		ang += 360.0f;
	
	return ang;
}

bool Map::check_appartenance(zone& z, vector3df point)
{
	// A zone is quad, a quad is composed from two triangles.
	// So, just need to check if a vertical line intersects one of the two triangles !!
	irr::core::triangle3df t1(z.getPoints()[0], z.getPoints()[1], z.getPoints()[3]);
	irr::core::triangle3df t2(z.getPoints()[2], z.getPoints()[1], z.getPoints()[3]);

	vector3df out;

	return t1.getIntersectionWithLine(point, vector3df(0.0f, 1.0f, 0.0f), out) ||
	       t2.getIntersectionWithLine(point, vector3df(0.0f, 1.0f, 0.0f), out);
}

bool Map::getZoneFromVector3df(vector3df point, zone& result)
{
	std::vector<int> zone_lst;
	std::vector<zone>::iterator it;

	int counter = 0;
	it = zones__.begin();
	while (it != zones__.end())
	{
		if (check_appartenance(*it, point))
			zone_lst.push_back(counter);
		++it;
		++counter;
	}

	if (zone_lst.size() == 0)
		return false;
	
	return getZoneFromHeight( zone_lst, point, result);
}

// FIXME: seems to be buggy, test this method
/*
int Map::getZoneFromVector3df(vector3df point)
{
	std::vector<int> zone_lst;
	std::vector<zone>::iterator it;

	int counter = 0;
	it = zones__.begin();
	while (it != zones__.end())
	{
		if (check_appartenance(*it, point))
			zone_lst.push_back(counter);
		++it;
		++counter;
	}

	if (zone_lst.empty())
		return -1;
	
	return getZoneFromHeight(zone_lst, point);
}
*/

bool Map::getZoneFromHeight( std::vector<int>& zone_lst, vector3df point, zone& result)
{
	std::vector<int>::iterator selected_it = zone_lst.end();
	float selected_Y;
	std::vector<int>::iterator it = zone_lst.begin();
	
	while (it != zone_lst.end())
	{
		vector3df out_intersect;
		bool collide;

		collide = zones__[*it].getPlane().getIntersectionWithLine(point, vector3df(0.0f, 1.0f, 0.0f), out_intersect);

		if (collide && out_intersect.Y < point.Y)
		{
			if (selected_it == zone_lst.end() || out_intersect.Y > selected_Y)
			{
				selected_Y = out_intersect.Y;
				selected_it = it;
			}
		}

		++it;
	}

	if (selected_it == zone_lst.end())
		return false;

	result = zones__[*selected_it];
	return true;
}

// FIXME: seems to be buggy, test this method
//int Map::getZoneFromHeight( std::vector<int>& zone_lst, vector3df point)
/*
{
	std::vector<int>::iterator selected_it = zone_lst.end();
	float selected_Y;
	std::vector<int>::iterator it = zone_lst.begin();

	while (it != zone_lst.end())
	{
		vector3df out_intersect;
		bool collide;

		collide = zones__[*it].getPlane().getIntersectionWithLine(point, vector3df(0.0f, 1.0f, 0.0f), out_intersect);

		if (collide && out_intersect.Y < point.Y)
		{
			if (selected_it == zone_lst.end() || out_intersect.Y > selected_Y)
			{
				selected_Y = out_intersect.Y;
				selected_it = it;
			}
		}

		++it;
	}

	if (selected_it == zone_lst.end())
		return -1;
	
	return zone_lst[*selected_it];
}
*/

std::list<segment_t> Map::getSegmentsToPassThrough(vector3df start_point, vector3df end_point)
{
	std::list<segment_t> seg_lst;

	zone start_zone;
	zone end_zone;


	if (!getZoneFromVector3df(start_point, start_zone) || !getZoneFromVector3df(end_point, end_zone))
		return seg_lst;
	if (start_zone == end_zone)
		return seg_lst;
	
	std::vector<int>::iterator it_s = start_zone.getSegments().begin();

	float travel_lenght = 0;
	float to_first_segment = 0;
	float from_last_segment = 0;

	int start_segment_selected = -1;
	int end_segment_selected = -1;
	float total_distance_selected = -1.0f;

	// Get the first and last segments index which covers the shortest path
	while (it_s != start_zone.getSegments().end())
	{
		// Compute distance from start_point to center of first segment
		segment_t& first_seg = segments__[ *it_s ];
		to_first_segment = first_seg.distance_to(start_point);

		std::vector<int>::iterator it_e = end_zone.getSegments().begin();
		while (it_e != end_zone.getSegments().end())
		{
			// Compute distance from end_point to center of last segment
			segment_t& last_seg = segments__[ *it_e ];
			from_last_segment = last_seg.distance_to(end_point);

			// Now get the distance to first segment and last segment
			travel_lenght = path_table__[ *it_s ][ *it_e ].second;

			if (travel_lenght + to_first_segment + from_last_segment < total_distance_selected || total_distance_selected < 0.0f)
			{
				total_distance_selected = travel_lenght + to_first_segment + from_last_segment;
				start_segment_selected = *it_s;
				end_segment_selected = *it_e;
			}

			it_e++;
		}
		it_s++;
	}

	// Now that we have the first and last segment, we must retrieve all intermediates segments
	int current_seg = end_segment_selected;
	while (current_seg != -1)
	{
		seg_lst.push_front(segments__[current_seg]);
		current_seg = path_table__[ start_segment_selected ][ current_seg ].first;
	}

	return seg_lst;
}

bool Map::getFarestSegment(vector3df origin, segment_t& out_segment)
{
	zone z;
	int selected_segment = -1;
	float selected_distance = -1.0f;
	if (getZoneFromVector3df(origin, z) == false)
		return false;
	
	std::vector<int>::iterator it_start = z.getSegments().begin();
	while (it_start != z.getSegments().end())
	{
		float distance_to_first_segment = segments__[*it_start].distance_to(origin);

		for (unsigned int i_end=0; i_end<path_table__[*it_start].size(); i_end++)
		{
			if (path_table__[*it_start][i_end].second + distance_to_first_segment > selected_distance)
			{
				selected_segment = i_end;
				selected_distance = path_table__[*it_start][i_end].second + distance_to_first_segment;
			}
		}
		++it_start;
	}

	if (selected_segment == -1)
		return false;

	out_segment = segments__[selected_segment];
	return true;
}

std::vector<zone>& Map::getZones()
{
	return zones__;
}

std::vector<segment_t>& Map::getSegments()
{
	return segments__;
}

// TODO: test this method
bool BBcollision(ISceneNode* first, ISceneNode* second)
{
	return BBcollision(first ->getTransformedBoundingBox().MinEdge,
			   first ->getTransformedBoundingBox().MaxEdge,
			   second->getTransformedBoundingBox().MinEdge,
			   second->getTransformedBoundingBox().MaxEdge);
}

bool BBcollision(ISceneNode* first, vector3df second_pos, double second_size)
{
	return BBcollision(first->getTransformedBoundingBox().MinEdge, first->getTransformedBoundingBox().MaxEdge, second_pos, second_size);
}

bool BBcollision(vector3df first_min, vector3df first_max, vector3df second_pos, double second_size)
{
	vector3df second_min = second_pos;
	second_min.X -= second_size;
	second_min.Y -= second_size;
	second_min.Z -= second_size;

	vector3df second_max = second_pos;
	second_max.X += second_size;
	second_max.Y += second_size;
	second_max.Z += second_size;

	return BBcollision(first_min, first_max, second_min, second_max);
}

bool BBcollision(vector3df first_min, vector3df first_max, vector3df second_min, vector3df second_max)
{
	if (second_max.X < first_min.X ||
	    second_min.X > first_max.X ||
	    second_max.Y < first_min.Y ||
	    second_min.Y > first_max.Y ||
	    second_max.Z < first_min.Z ||
	    second_min.Z > first_max.Z)
		return false;

	return true;
}

